clear all
%close all

springN		= 1;
save_plots	= 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

log_name = sprintf('./zero_trq_test.no.2950.%d',springN);

str = sprintf('../../../jhurst/current/Kemper_journal/SEDA_model/spring%d/sys%d.mat',springN,springN);
load(str);


tauc	= sys.tauc;
GR		= sys.GR;


K		= sys.K;
B		= sys.B;
I		= sys.I;
OFFSET	= sys.OFFSET+0.2085;

A2T = tauc*GR;

t_start = 3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Run controllers

cnts = 2300;

f = cnts/512;

t_end	= t_start+10*1/f;

w		= 2*pi*f;
kP		= 0;
kD		= 0;
kF		= 0;
trq_lim = 15;
trq_max = 5;


theta_a = 250.0380e-003/2;
w0	= sqrt(K/I);
z	= B/(2*sqrt(K*I));
wd	= w0*sqrt(1-z^2);
fd	= wd/(2*pi)

f_worst = (1/(2*pi))*sqrt(trq_lim/(theta_a*I))

cnts = f_worst * 512

ctrl.kP			= kP;
ctrl.kD			= kD;
ctrl.kF			= kF;
ctrl.trq_lim	= trq_lim;
ctrl.trq_max	= trq_max;
str = sprintf('../../../jhurst/current/Kemper_journal/SEDA_model/spring%d/ctrl_ft%d.mat',springN,springN);
save(str,'ctrl');



fprintf('Running controller:\n');
fprintf('Command Frequency: %f\n',f);
%			controller	log_file	time_start	time_stop	[controller_parms]
cmd_str = sprintf('sudo ./bin/fca_interface_headless 3 %s.log %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f', log_name, t_start, t_end, 0, 2.8, 1, trq_lim, kP, kD, kF, 0, 0);


fprintf('%s\n\n',cmd_str);

ret = unix(cmd_str);
if (ret ~= 0)
	fprintf('Controller fail!\n');
	return;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compile logs

fprintf('Compiling logs\n');

fprintf('Opening log:\t');
fprintf('%s.log\n',log_name);

log_str = sprintf('%s.log',log_name);
log = importdata(log_str);

motor_angle		= log.data(:,4);
leg_angle		= log.data(:,5);

motor_vel		= log.data(:,6);
leg_vel			= log.data(:,7);

motor_current	= log.data(:,8);


t = log.data(:,1);

mtr_trq = motor_current*A2T;

spring_pos = motor_angle-leg_angle + OFFSET;
spring_vel = motor_vel - leg_vel;

% spring_trq = K*spring_pos + B*spring_vel;
spring_trq = K*spring_pos;

W = ceil(length(t)*0.03);
if (W>10)
	W = 10;
end

temporary	= filter( repmat( 1/2/W, 1, W ), 1, spring_trq( end : -1 : 1, : ) );
filt_s		= filter( repmat( 1/2/W, 1, W ), 1, spring_trq ) ...
			+  temporary( end : -1 : 1, : );

spring_trq = filt_s;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h = figure;
set(h,'position',[ 100 100 400 400])
hold on
% str = sprintf('Spring %d: Torques',springN);
% title(str)

plot([t(1) t(end)], [trq_max trq_max], 'k--','linewidth', 1);
plot([t(1) t(end)], [-trq_max -trq_max], 'k--','linewidth', 1);
plot([t(1) t(end)], [0 0], 'k--','linewidth', 1);

pt = plot(t,mtr_trq,'g','linewidth', 2);

pm = plot(t,spring_trq,'linewidth', 2);


legend([pt; pm],'Motor', 'Load')

axis tight

xlabel(	'\textbf{Time} $\left( s \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

ylabel(	'\textbf{Torque} $\left( N \cdot m \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

set(gca,'Position',[ 0.15 0.15 0.8 0.8])
	
str = sprintf('/home/drl/projects/jhurst/current/Kemper_journal/images/track%d.pdf', springN);
if (save_plots == 1)
	save2pdf(str);
end


figure
plot([motor_angle+OFFSET leg_angle]), legend('Motor','Leg')
